#!/usr/bin/env python3

import rospy
from std_msgs.msg import Int32
import time


def send_command():
    rospy.init_node('send_command_node', anonymous=True)
    pub = rospy.Publisher('/command', Int32, queue_size=1)

    rospy.loginfo("Send Command Node Initialized.")
    rospy.loginfo("Commands: 0=INIT, 1=ARM, 2=TAKEOFF, 4=RESET_POS, 5=START_IBVS_TRACKING, 3=LAND, 7=WAITING")
    rospy.loginfo("Type command number and press Enter. Type 'q' to quit.")

    rate = rospy.Rate(1)  # 1 Hz

    while not rospy.is_shutdown():
        try:
            cmd_input = input("Enter command: ")
            if cmd_input.lower() == 'q':
                break

            cmd_int = int(cmd_input)
            msg = Int32()
            msg.data = cmd_int

            # Publish a few times to ensure the controller receives it
            for _ in range(3):
                pub.publish(msg)
                rospy.loginfo(f"Sent command: {cmd_int}")
                time.sleep(0.1)  # Small delay

        except ValueError:
            rospy.logwarn("Invalid input. Please enter a number or 'q'.")
        except Exception as e:
            rospy.logerr(f"An error occurred: {e}")

        rate.sleep()


if __name__ == '__main__':
    try:
        send_command()
    except rospy.ROSInterruptException:
        pass
